#include "util.h"
//#include "Math3d.h"

namespace LibWiimote{

vect::vect():x(0), y(0), z(0){};
vect::vect(float _x, float _y, float _z): x(_x), y(_y), z(_z)
{
	ComputeSpherical();
}
vect::vect(const vect & v):x(v.x), y(v.y), z(v.z){
	ComputeSpherical();
}

void vect::ComputeSpherical(){
	rho = sqrtf(x*x+y*y+z*z);
	if (rho>0.1){
		phi = acos(z/rho);
		if (y>=0.0f){
			theta = acos(x/sqrtf(x*x+y*y));
		}else{
			theta = 6.2831853f - acos(x/sqrtf(x*x+y*y));
		}
	}else{
		phi = 0.0f;
		theta = 0.0f;
	}
}

void vect::ComputeCartesian(){
	x = rho * sin(phi) * cos(theta);
	y = rho * sin(phi) * sin(theta);
	z = rho * cos(phi);
}

std::ostream & operator << (std::ostream & stream, vect & vect){
	if (vect.spherical_output)
		stream << vect.rho << " " << vect.phi << " " << vect.theta;
	else
		stream << vect.x << " " << vect.y << " " << vect.z;

	return stream;
}

vect vect::operator+(const vect &rhs){
	return vect(x+rhs.x, y+rhs.y, z+rhs.z);
}
vect vect::operator-(const vect &rhs){
	return vect(x-rhs.x, y-rhs.y, z-rhs.z);
}

vect operator*(float f, vect & v){
	return vect(v.x*f, v.y*f, v.z*f);
}

float vect::length(){
	return sqrt(x*x+y*y+z*z);
}

void quaternion::init(vect dir, float angle){
	float sin_a = sin(angle / 2.0f);
	float cos_a = cos(angle / 2.0f);

	x = dir.x * sin_a;
	y = dir.y * sin_a;
	z = dir.z * sin_a;
	w = cos_a;

	normalize();
}

void quaternion::init(float yaw, float pitch, float roll){
	//const float fSinPitch(sin(pitch*0.5F));
	//const float fCosPitch(cos(pitch*0.5F));
	//const float fSinYaw(sin(yaw*0.5F));
	//const float fCosYaw(cos(yaw*0.5F));
	//const float fSinRoll(sin(roll*0.5F));
	//const float fCosRoll(cos(roll*0.5F));
	//const float fCosPitchCosYaw(fCosPitch*fCosYaw);
	//const float fSinPitchSinYaw(fSinPitch*fSinYaw);
	//x = fSinRoll * fCosPitchCosYaw     - fCosRoll * fSinPitchSinYaw;
	//y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
	//z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
	//w = fCosRoll * fCosPitchCosYaw     + fSinRoll * fSinPitchSinYaw;


	const float halfYaw = (yaw) * (0.5);  
	const float halfPitch = (pitch) * (0.5);  
	const float halfRoll = (roll) * (0.5);  
	const float cosYaw = cos(halfYaw);
	const float sinYaw = sin(halfYaw);
	const float cosPitch = cos(halfPitch);
	const float sinPitch = sin(halfPitch);
	const float cosRoll = cos(halfRoll);
	const float sinRoll = sin(halfRoll);
	x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
	y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
	z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
	w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
}

void quaternion::normalize(){
	float n = norm();
	x/=n;
	y/=n;
	z/=n;
	w/=n;
}

float quaternion::norm(){
	return sqrt(w * w + x * x + y * y + z * z);
}


void matrix::init(quaternion q){
	float xx      = q.x * q.x;
	float xy      = q.x * q.y;
	float xz      = q.x * q.z;
	float xw      = q.x * q.w;

	float yy      = q.y * q.y;
	float yz      = q.y * q.z;
	float yw      = q.y * q.w;

	float zz      = q.z * q.z;
	float zw      = q.z * q.w;

	mat[0]  = 1.0f - 2.0f * ( yy + zz );
	mat[1]  =        2.0f * ( xy - zw );
	mat[2]  =        2.0f * ( xz + yw );

	mat[4]  =        2.0f * ( xy + zw );
	mat[5]  = 1.0f - 2.0f * ( xx + zz );
	mat[6]  =        2.0f * ( yz - xw );

	mat[8]  =        2.0f * ( xz - yw );
	mat[9]  =        2.0f * ( yz + xw );
	mat[10] = 1.0f - 2.0f * ( xx + yy );

	mat[3]  = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0.0;
	mat[15] = 1.0;
}

void matrix::init(float * v){
	memcpy(mat, v, 16*sizeof(float));
}
matrix::matrix(quaternion q){init(q);}
matrix::matrix(float * v){init(v);}
matrix::matrix(){}
matrix::matrix(matrix & m){init(m.get());}

vect matrix::mult(vect v){
	vect r;
	r.x = v.x*mat[ 0] + v.y*mat[ 1]+v.z*mat[ 2];
	r.y = v.x*mat[ 4] + v.y*mat[ 5]+v.z*mat[ 6];
	r.z = v.x*mat[ 8] + v.y*mat[ 9]+v.z*mat[10];
	return r;
};

float * matrix::get(){
	return mat;
}

matrix operator*(matrix &a, matrix &b )
{
	matrix res;

	// Compute the left 4x3 (linear transformation) portion
	res._m11 = (a._m11 * b._m11) + (a._m21 * b._m12) + (a._m31 * b._m13);
	res._m12 = (a._m12 * b._m11) + (a._m22 * b._m12) + (a._m32 * b._m13);
	res._m13 = (a._m13 * b._m11) + (a._m23 * b._m12) + (a._m33 * b._m13);

	res._m21 = (a._m11 * b._m21) + (a._m21 * b._m22) + (a._m31 * b._m23);
	res._m22 = (a._m12 * b._m21) + (a._m22 * b._m22) + (a._m32 * b._m23);
	res._m23 = (a._m13 * b._m21) + (a._m23 * b._m22) + (a._m33 * b._m23);

	res._m31 = (a._m11 * b._m31) + (a._m21 * b._m32) + (a._m31 * b._m33);
	res._m32 = (a._m12 * b._m31) + (a._m22 * b._m32) + (a._m32 * b._m33);
	res._m33 = (a._m13 * b._m31) + (a._m23 * b._m32) + (a._m33 * b._m33);

	// Compute the translation portion
	res._tx = (a._m11 * b._tx) + (a._m21 * b._ty) + (a._m31 * b._tz) + a._tx;
	res._ty = (a._m12 * b._tx) + (a._m22 * b._ty) + (a._m32 * b._tz) + a._ty;
	res._tz = (a._m13 * b._tx) + (a._m23 * b._ty) + (a._m33 * b._tz) + a._tz;

	return res;
}

void matrix::setOrigin(vect & v){
	_tx = v.x; 
	_ty = v.y;
	_tz = v.z;
}

}